/**
 * Senior project: Augmented reality
 * main.cpp
 */

#ifdef __APPLE__
# include <OpenGL/gl.h>
# include <OpenGL/glu.h>
# include <GLUT/glut.h>
#else
# include <GL/gl.h>
# include <GL/glu.h>
# include <GL/glut.h>
#endif

#include "aruco.h"
#include "imagehandler.h"

// global variables
const int MAX_FRAME_RATE = 60;

// window size
int windowWidth = 640;
int windowHeight = 480;

// image processing
ImageHandler theImageHandler;
vector<aruco::Marker> theDetectedMarkers;
Mat previousFrame = Mat();
Mat backgroundFrame;
Mat frameToDraw;
Rect boundedMarker;
vector<Point2f> *previousPoints = new vector<Point2f>();
vector<Point2f> *nextPoints = new vector<Point2f>();
vector<vector<cv::Point> > previousFrameContours;

bool markersLocked = false;
int curveApproxType = 0;
int thresholdValue = 25;

// texture to draw on detected markers
GLuint theVideoTexture;
GLuint theVirtualTexture;
Mat virtualTexture;

// for contour finding in the background subtraction
std::vector<std::vector<cv::Point> > contours;
std::vector<cv::Vec4i> hierarchy;


void drawCameraFrame() {
    // draw a quad and texture it with the frame from the camera
    glTexImage2D(GL_TEXTURE_2D,
                 0,
                 GL_RGB,
                 frameToDraw.size().width,
                 frameToDraw.size().height,
                 0,
                 GL_RGB,
                 GL_UNSIGNED_BYTE,
                 frameToDraw.data);
    
    glColor3f(1, 1, 1);
    glEnable(GL_TEXTURE_2D);
    glBegin(GL_QUADS);
    glTexCoord2f(0, 0); glVertex2f(0, 0);
    glTexCoord2f(1, 0); glVertex2f(windowWidth, 0);
    glTexCoord2f(1, 1); glVertex2f(windowWidth, windowHeight);
    glTexCoord2f(0, 1); glVertex2f(0, windowHeight);
    glEnd();
    glDisable(GL_TEXTURE_2D);
}

void drawMarkers() {
    for (int i = 0; i < theDetectedMarkers.size(); i++) {
        // draw and texture a quad overlaying the marker
        glTexImage2D(GL_TEXTURE_2D,
                     0,
                     GL_RGB,
                     virtualTexture.size().width,
                     virtualTexture.size().height,
                     0,
                     GL_RGB,
                     GL_UNSIGNED_BYTE,
                     virtualTexture.data);
        glEnable(GL_TEXTURE_2D);
        glBegin(GL_QUADS);
        for (int j = 0; j < 4; j++) {
            // in aruco, a Marker object is a vector of 4 points
            switch (j) {
                case 0:
                    glTexCoord2f(0, 0);
                    break;
                case 1:
                    glTexCoord2f(1, 0);
                    break;
                case 2:
                    glTexCoord2f(1, 1);
                    break;
                case 3:
                    glTexCoord2f(0, 1);
                    break;
                default:
                    break;
            }
            glVertex2f(theDetectedMarkers.at(i)[j].x,
                       theDetectedMarkers.at(i)[j].y);
        }
        glEnd();
        glDisable(GL_TEXTURE_2D);
        
        // Here is where we draw the occlusion related stuff.
        if (markersLocked) {
            glTexImage2D(GL_TEXTURE_2D,
                         0,
                         GL_RGB,
                         frameToDraw.size().width,
                         frameToDraw.size().height,
                         0,
                         GL_RGB,
                         GL_UNSIGNED_BYTE,
                         frameToDraw.data);
            
            for (int i = 0; i < contours.size(); i++) {
                Mat curve(contours[i]);
                vector<cv::Point> approxCurve;
                
                // Get points of occlusion mask
                if (curveApproxType == 0) {
                    approxPolyDP(curve, approxCurve, 0.9, false);
                } else if (curveApproxType == 1) {
                    convexHull(curve, approxCurve);
                }
                
                // Create occlusion mask for OpenGL and texture it
                glEnable(GL_TEXTURE_2D);
                glBegin(GL_POLYGON);
                for (int j = 0; j < approxCurve.size(); j++) {
                    // UGH UGH UGH BRAD I AM SO SORRY FOR THESE CASTS
                    glTexCoord2d(((double)approxCurve[j].x + boundedMarker.x)/640, ((double)approxCurve[j].y + boundedMarker.y)/480);
                    glVertex2d((double)approxCurve[j].x + boundedMarker.x, (double)approxCurve[j].y + boundedMarker.y);
                }
                glEnd();
                glDisable(GL_TEXTURE_2D);
            }
        }
    }
}

/**
 * Timer callback. Performs marker detection step.
 */
void timerCallback(int value) {
    int nextValue = value;
    frameToDraw = theImageHandler.getNextFrame();
    Mat roi;
    
    vector<aruco::Marker> matchesResults = theImageHandler.findTemplateMatches(frameToDraw);
    
    if (!matchesResults.empty() || !markersLocked) {
        theDetectedMarkers = matchesResults;
        
        if (!theDetectedMarkers.empty()) {
            // Markers found!
            // get the rectangle where the marker is
            Point2f pointsToBound[] = {Point2d(theDetectedMarkers.at(0)[0].x, theDetectedMarkers.at(0)[0].y),
                Point2f(theDetectedMarkers.at(0)[1].x, theDetectedMarkers.at(0)[1].y),
                Point2f(theDetectedMarkers.at(0)[2].x, theDetectedMarkers.at(0)[2].y),
                Point2f(theDetectedMarkers.at(0)[3].x, theDetectedMarkers.at(0)[3].y)};
            vector<Point2f> pointsToBoundVec (pointsToBound, pointsToBound + sizeof(pointsToBound) / sizeof(Point2f) );
            Mat pointsToBoundMat(pointsToBoundVec);
            boundedMarker = Rect(cv::boundingRect(pointsToBoundMat));
            
            // find the region of interest--that is, where the marker is (just one)
            roi = Mat(frameToDraw, boundedMarker);
            
            // Set the background frame
            backgroundFrame = roi.clone();
            
//            namedWindow("background");
//            imshow("background", backgroundFrame);
//            namedWindow("roi");
//            imshow("roi", roi);
        }
    }
    
    if (!backgroundFrame.empty()) {
        // look at the region of interest on this new frame
        Mat roi(frameToDraw, boundedMarker);
        Mat diffedFrame = theImageHandler.findDiff(backgroundFrame, roi, thresholdValue);
        
        namedWindow("diffed");
        imshow("diffed", diffedFrame);
        
        cv::findContours(diffedFrame,
                         contours,
                         hierarchy,
                         CV_RETR_EXTERNAL,
                         CV_CHAIN_APPROX_NONE);
    }
    
    previousFrame = frameToDraw;
    
    // tell opengl to draw, then reset the timer
    glutPostRedisplay();
    glutTimerFunc(1000/MAX_FRAME_RATE, timerCallback, nextValue);
}

/**
 * Display callback that is called every time there is something to draw onscreen
 */
void displayCallback() {
    // some initialization before drawing
    glClearColor(0,0,0,0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
    // Set up viewport...
    glViewport(0, 0, windowWidth, windowHeight);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, windowWidth, windowHeight, 0, -1, 1);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    
    drawCameraFrame();
    drawMarkers();
    
    glutSwapBuffers();
}

/**
 * Reshape callback is run every time the window is resized
 */
void reshapeCallback(int w, int h) {
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    
    // Set up a perspective projection based on the window's aspect ratio
    //gluPerspective(45.0f, (GLfloat)w/(GLfloat)h, 0.1f, 750);
}

/**
 * Keypress handling
 */
void keyDownCallback(unsigned char key, int x, int y) {
    switch (key) {
        case 'l':
            /* Lock markers */
            if (markersLocked) {
                std::cout << "unlocking markers" << std::endl;
                backgroundFrame.release();
                markersLocked = false;
            } else {
                std::cout << "locking markers" << std::endl;
                markersLocked = true;
            }
            break;
        case 'p':
            /* Increase threshold for background subtraction */
            if (thresholdValue < 200) {
                thresholdValue++;
            }
            std::cout << "thresholdValue = " << thresholdValue << std::endl;
            break;
        case 'o':
            /* Decrease threshold for background subtraction */
            if (thresholdValue > 0) {
                thresholdValue--;
            }
            std::cout << "thresholdValue = " << thresholdValue << std::endl;
            break;
        case 27:
            // Escape key: exit program
            exit(0);
            break;
        default:
            break;
    }
}

int main (int argc, char **argv) {
    // glut window set-up
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE);
    glutInitWindowSize(windowWidth, windowHeight);
    glutCreateWindow("AR");
    glutTimerFunc(1000/60, timerCallback, 0);
    glutDisplayFunc(displayCallback);
    glutReshapeFunc(reshapeCallback);
    glutKeyboardFunc(keyDownCallback);
    
    //glDisable(GL_DEPTH_TEST);
    glViewport(0, 0, windowWidth, windowHeight);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, windowWidth, windowHeight, 0, -1, 1);
    glMatrixMode(GL_MODELVIEW);             
    glLoadIdentity();
    
    // prepare the video texture
    glGenTextures(1, &theVideoTexture);
    glBindTexture(GL_TEXTURE_2D, theVideoTexture);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);      // byte alignment
    
    glGenTextures(1, &theVirtualTexture);
    glBindTexture(GL_TEXTURE_2D, theVirtualTexture);
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);      // byte alignment
    
    // default texture parameters
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
    
    // load texture for virtual object
    virtualTexture = imread("teapot.jpg");
    cvtColor(virtualTexture, virtualTexture, CV_BGR2RGB);
    
    // Begin main program loop
    glutMainLoop();
    
    return 0;
}
